#include <rclcpp/rclcpp.hpp>
#include <geometry_msgs/msg/twist.hpp>
#include <sensor_msgs/msg/joy.hpp>

class TeleopTwistJoyNode : public rclcpp::Node
{
public:
  TeleopTwistJoyNode() : Node("pb_teleop_twist_joy_node")
  {
    // Declare parameters with default values
    this->declare_parameter("enable_button", 0);
    this->declare_parameter("enable_turbo_button", -1);
    
    this->declare_parameter("axis_linear.x", 1);
    this->declare_parameter("axis_linear.y", 0);
    this->declare_parameter("axis_linear.z", -1);
    this->declare_parameter("axis_angular.yaw", 2);
    this->declare_parameter("axis_angular.pitch", -1);
    this->declare_parameter("axis_angular.roll", -1);
    
    this->declare_parameter("scale_linear", 0.5);
    this->declare_parameter("scale_linear_turbo", 1.0);
    this->declare_parameter("scale_angular", 1.0);
    this->declare_parameter("scale_angular_turbo", 1.0);
    
    this->declare_parameter("require_enable_button", true);
    
    // Initialize subscribers and publishers
    joy_sub_ = this->create_subscription<sensor_msgs::msg::Joy>(
      "joy", 10, std::bind(&TeleopTwistJoyNode::joyCallback, this, std::placeholders::_1));
    
    cmd_vel_pub_ = this->create_publisher<geometry_msgs::msg::Twist>("cmd_vel", 10);
    
    RCLCPP_INFO(this->get_logger(), "PolarBear Teleop Twist Joy Node started");
  }

private:
  void joyCallback(const sensor_msgs::msg::Joy::SharedPtr joy_msg)
  {
    // Get parameters
    int enable_button = this->get_parameter("enable_button").as_int();
    int enable_turbo_button = this->get_parameter("enable_turbo_button").as_int();
    bool require_enable_button = this->get_parameter("require_enable_button").as_bool();
    
    // Check if enable button is pressed (if required)
    bool enabled = !require_enable_button;
    if (require_enable_button && enable_button >= 0 && enable_button < (int)joy_msg->buttons.size()) {
      enabled = joy_msg->buttons[enable_button];
    }
    
    if (!enabled) {
      // Publish zero twist
      auto cmd_vel = geometry_msgs::msg::Twist();
      cmd_vel_pub_->publish(cmd_vel);
      return;
    }
    
    // Check turbo mode
    bool turbo_mode = false;
    if (enable_turbo_button >= 0 && enable_turbo_button < (int)joy_msg->buttons.size()) {
      turbo_mode = joy_msg->buttons[enable_turbo_button];
    }
    
    // Get axis indices
    int axis_linear_x = this->get_parameter("axis_linear.x").as_int();
    int axis_linear_y = this->get_parameter("axis_linear.y").as_int();
    int axis_linear_z = this->get_parameter("axis_linear.z").as_int();
    int axis_angular_yaw = this->get_parameter("axis_angular.yaw").as_int();
    
    // Get scales
    double scale_linear = turbo_mode ? 
      this->get_parameter("scale_linear_turbo").as_double() : 
      this->get_parameter("scale_linear").as_double();
    double scale_angular = turbo_mode ? 
      this->get_parameter("scale_angular_turbo").as_double() : 
      this->get_parameter("scale_angular").as_double();
    
    // Create twist message
    auto cmd_vel = geometry_msgs::msg::Twist();
    
    // Set linear velocities
    if (axis_linear_x >= 0 && axis_linear_x < (int)joy_msg->axes.size()) {
      cmd_vel.linear.x = joy_msg->axes[axis_linear_x] * scale_linear;
    }
    if (axis_linear_y >= 0 && axis_linear_y < (int)joy_msg->axes.size()) {
      cmd_vel.linear.y = joy_msg->axes[axis_linear_y] * scale_linear;
    }
    if (axis_linear_z >= 0 && axis_linear_z < (int)joy_msg->axes.size()) {
      cmd_vel.linear.z = joy_msg->axes[axis_linear_z] * scale_linear;
    }
    
    // Set angular velocity
    if (axis_angular_yaw >= 0 && axis_angular_yaw < (int)joy_msg->axes.size()) {
      cmd_vel.angular.z = joy_msg->axes[axis_angular_yaw] * scale_angular;
    }
    
    // Publish twist
    cmd_vel_pub_->publish(cmd_vel);
  }
  
  rclcpp::Subscription<sensor_msgs::msg::Joy>::SharedPtr joy_sub_;
  rclcpp::Publisher<geometry_msgs::msg::Twist>::SharedPtr cmd_vel_pub_;
};

int main(int argc, char** argv)
{
  rclcpp::init(argc, argv);
  
  auto node = std::make_shared<TeleopTwistJoyNode>();
  
  try {
    rclcpp::spin(node);
  } catch (const std::exception& e) {
    RCLCPP_ERROR(rclcpp::get_logger("pb_teleop_twist_joy"), "Exception: %s", e.what());
  }
  
  rclcpp::shutdown();
  return 0;
} 